using System;
using System.Diagnostics;
using BepuUtilities;
using System.Runtime.CompilerServices;
using BepuPhysics.CollisionDetection;
using BepuUtilities.Memory;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Collidables
{
    public struct HullBoundingPlanes
    {
        /// <summary>
        /// 边界平面的法线。
        /// </summary>
        public Vector3Wide Normal;
        /// <summary>
        /// 沿法线从原点到平面上一点的偏移。
        /// </summary>
        public Vector<float> Offset;
    }


    public struct HullVertexIndex
    {
        // 这意味着凸壳中只能有向量<浮点>.Count*65536个点。哦不！
        public ushort BundleIndex;
        public ushort InnerIndex;

        public override string ToString()
        {
            return $"({BundleIndex}, {InnerIndex})";
        }
    }

    public struct ConvexHull : IConvexShape
    {
        /// <summary>
        /// 凸包的集合点。
        /// </summary>
        public Buffer<Vector3Wide> Points;
        /// <summary>
        /// 凸壳的捆绑边界平面。
        /// </summary>
        public Buffer<HullBoundingPlanes> BoundingPlanes;
        /// <summary>
        /// 每个面使用的一组组合顶点。使用FaceToVertexIndicesStart为特定面编制索引。在右手空间中按逆时针缠绕存储索引,在左手空间中按顺时针方向存储索引。
        /// </summary>
        public Buffer<HullVertexIndex> FaceVertexIndices;
        /// <summary>
        /// FaceVertexIndices中面的起始索引。
        /// </summary>
        public Buffer<int> FaceToVertexIndicesStart;

        /// <summary>
        /// 从点集创建凸面外壳线。
        /// </summary>
        /// <param name="Points">计算的凸包的点数。</param>
        /// <param name="pool">要在其中分配凸面外壳和计算外壳所需的任何临时资源的池。</param>
        /// <param name="center">重新对中凸壳之前计算的凸壳中心。</param>
        public ConvexHull(Span<Vector3> points, BufferPool pool, out Vector3 center)
        {
            ConvexHullHelper.CreateShape(points, pool, out center, out this);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetVertexIndicesForFace(int faceIndex, out Buffer<HullVertexIndex> faceVertexIndices)
        {
            var start = FaceToVertexIndicesStart[faceIndex];
            var nextFaceIndex = faceIndex + 1;
            var end = nextFaceIndex == FaceToVertexIndicesStart.Length ? FaceVertexIndices.Length : FaceToVertexIndicesStart[nextFaceIndex];
            var count = end - start;
            FaceVertexIndices.Slice(start, count, out faceVertexIndices);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetPoint(HullVertexIndex pointIndex, out Vector3 point)
        {
            Vector3Wide.ReadSlot(ref Points[pointIndex.BundleIndex], pointIndex.InnerIndex, out point);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetPoint(int pointIndex, out Vector3 point)
        {
            BundleIndexing.GetBundleIndices(pointIndex, out var bundleIndex, out var innerIndex);
            Vector3Wide.ReadSlot(ref Points[bundleIndex], innerIndex, out point);

        }

        // TODO：使用平台内部特性,我们可以改进这些函数的"水平"部分。
        public void ComputeAngularExpansionData(out float maximumRadius, out float maximumAngularExpansion)
        {
            Vector<float> maximumRadiusSquaredWide = default;
            Vector<float> minimumRadiusSquaredWide = new Vector<float>(float.MaxValue);
            for (int i = 0; i < Points.Length; ++i)
            {
                Vector3Wide.LengthSquared(Points[i], out var candidate);
                maximumRadiusSquaredWide = Vector.Max(candidate, maximumRadiusSquaredWide);
                minimumRadiusSquaredWide = Vector.Min(candidate, minimumRadiusSquaredWide);
            }
            var minimumRadiusWide = Vector.SquareRoot(minimumRadiusSquaredWide);
            var maximumRadiusWide = Vector.SquareRoot(maximumRadiusSquaredWide);
            maximumRadius = maximumRadiusWide[0];
            float minimumRadius = minimumRadiusWide[0];
            for (int i = 1; i < Vector<float>.Count; ++i)
            {
                var maxCandidate = maximumRadiusWide[i];
                var minCandidate = minimumRadiusWide[i];
                if (maxCandidate > maximumRadius)
                    maximumRadius = maxCandidate;
                if (minCandidate < minimumRadius)
                    minimumRadius = minCandidate;
            }
            maximumAngularExpansion = maximumRadius - minimumRadius;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal void ComputeBounds(in QuaternionWide orientationWide, out Vector3 min, out Vector3 max)
        {
            Matrix3x3Wide.CreateFromQuaternion(orientationWide, out var orientationMatrix);
            Vector3Wide minWide = default, maxWide = default;
            for (int i = 0; i < Points.Length; ++i)
            {
                Matrix3x3Wide.TransformWithoutOverlap(Points[i], orientationMatrix, out var p);
                Vector3Wide.Min(minWide, p, out minWide);
                Vector3Wide.Max(maxWide, p, out maxWide);
            }
            Vector3Wide.ReadFirst(minWide, out min);
            Vector3Wide.ReadFirst(maxWide, out max);
            for (int i = 1; i < Vector<float>.Count; ++i)
            {
                // TODO：检查codegen。边界检查被取消了吗？
                var minCandidate = new Vector3(minWide.X[i], minWide.Y[i], minWide.Z[i]);
                var maxCandidate = new Vector3(maxWide.X[i], maxWide.Y[i], maxWide.Z[i]);
                min = Vector3.Min(minCandidate, min);
                max = Vector3.Max(maxCandidate, max);
            }
        }

        public void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max)
        {
            QuaternionWide.Broadcast(orientation, out var orientationWide);
            ComputeBounds(orientationWide, out min, out max);
        }

        public struct ConvexHullTriangleSource : ITriangleSource
        {
            ConvexHull hull;
            int faceIndex;
            int subtriangleIndex;

            public ConvexHullTriangleSource(in ConvexHull hull)
            {
                this.hull = hull;
                faceIndex = 0;
                subtriangleIndex = 2;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool GetNextTriangle(out Vector3 a, out Vector3 b, out Vector3 c)
            {
                // 这不是很直接或快速,但它相当简单,不需要冗余的实现。
                if (faceIndex < hull.FaceToVertexIndicesStart.Length)
                {
                    hull.GetVertexIndicesForFace(faceIndex, out var faceIndices);
                    hull.GetPoint(faceIndices[0], out a);
                    // 注意c和b的翻转;MeshInertiaHelper需要在右手坐标中逆时针外部可见的三角形。
                    // ConvexHull无缘无故地有相反的约定。
                    hull.GetPoint(faceIndices[subtriangleIndex - 1], out c);
                    hull.GetPoint(faceIndices[subtriangleIndex], out b);
                    ++subtriangleIndex;
                    if (subtriangleIndex == faceIndices.Length)
                    {
                        subtriangleIndex = 2;
                        ++faceIndex;
                    }
                    return true;
                }
                a = default;
                b = default;
                c = default;
                return false;
            }
        }

        /// <summary>
        /// 计算凸壳的惯性。
        /// </summary>
        /// <param name="quality">用于缩放惯性张量的质量。</param>
        /// <param name="惯性">凸壳的惯性。</param>
        public void ComputeInertia(float mass, out BodyInertia inertia)
        {
            var triangleSource = new ConvexHullTriangleSource(this);
            MeshInertiaHelper.ComputeClosedInertia(ref triangleSource, mass, out _, out var inertiaTensor);
            inertia.InverseMass = 1f / mass;
            Symmetric3x3.Invert(inertiaTensor, out inertia.InverseInertiaTensor);
        }

        public ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches)
        {
            return new ConvexHullShapeBatch(pool, initialCapacity);
        }

        public bool RayTest(in RigidPose pose, in Vector3 origin, in Vector3 direction, out float t, out Vector3 normal)
        {
            Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientation);
            var shapeToRay = origin - pose.Position;
            Matrix3x3.TransformTranspose(shapeToRay, orientation, out var localOrigin);
            Matrix3x3.TransformTranspose(direction, orientation, out var localDirection);
            Vector3Wide.Broadcast(localOrigin, out var localOriginBundle);
            Vector3Wide.Broadcast(localDirection, out var localDirectionBundle);

            Helpers.FillVectorWithLaneIndices(out var indexOffsets);
            // 光线上的相交间隔是它进入所有边界平面之后、离开其中任何边界平面之前的时间。
            // 所有面法线都指向外部。
            var latestEntryNumeratorBundle = new Vector<float>(float.MaxValue);
            var latestEntryDenominatorBundle = new Vector<float>(-1);
            var latestEntryIndexBundle = new Vector<int>();
            var earliestExitNumeratorBundle = new Vector<float>(float.MaxValue);
            var earliestExitDenominatorBundle = new Vector<float>(1);
            for (int i = 0; i < BoundingPlanes.Length; ++i)
            {
                ref var boundingPlane = ref BoundingPlanes[i];
                var candidateIndices = new Vector<int>(i << BundleIndexing.VectorShift) + indexOffsets;
                // t=点(pointOnPlane-原点,平面法线)/点(平面法线,光线方向)
                // 请注意,我们可以推迟除法;我们不需要计算*所有*平面的确切t值。
                Vector3Wide.Dot(localOriginBundle, boundingPlane.Normal, out var normalDotOrigin);
                var numerator = boundingPlane.Offset - normalDotOrigin;
                Vector3Wide.Dot(localDirectionBundle, boundingPlane.Normal, out var denominator);
                // 如果光线方向与面法线相反,则边界平面将被"输入"。
                // 进入分母始终为负,退出分母始终为正。不用担心比较符号翻转。
                // 如果分母为零,则忽略车道。
                var useLatestEntryCandidate = Vector.BitwiseAnd(Vector.LessThan(denominator, Vector<float>.Zero), Vector.GreaterThan(numerator * latestEntryDenominatorBundle, latestEntryNumeratorBundle * denominator));
                var useEarliestExitCandidate = Vector.BitwiseAnd(Vector.GreaterThan(denominator, Vector<float>.Zero), Vector.LessThan(numerator * earliestExitDenominatorBundle, earliestExitNumeratorBundle * denominator));
                latestEntryNumeratorBundle = Vector.ConditionalSelect(useLatestEntryCandidate, numerator, latestEntryNumeratorBundle);
                latestEntryDenominatorBundle = Vector.ConditionalSelect(useLatestEntryCandidate, denominator, latestEntryDenominatorBundle);
                latestEntryIndexBundle = Vector.ConditionalSelect(useLatestEntryCandidate, candidateIndices, latestEntryIndexBundle);
                earliestExitNumeratorBundle = Vector.ConditionalSelect(useEarliestExitCandidate, numerator, earliestExitNumeratorBundle);
                earliestExitDenominatorBundle = Vector.ConditionalSelect(useEarliestExitCandidate, denominator, earliestExitDenominatorBundle);
            }
            var latestEntryNumerator = latestEntryNumeratorBundle[0];
            var latestEntryDenominator = latestEntryDenominatorBundle[0];
            var latestEntryIndex = latestEntryIndexBundle[0];
            var earliestExitNumerator = earliestExitNumeratorBundle[0];
            var earliestExitDenominator = earliestExitDenominatorBundle[0];
            for (int i = 1; i < Vector<float>.Count; ++i)
            {
                var latestEntryNumeratorCandidate = latestEntryNumeratorBundle[i];
                var latestEntryDenominatorCandidate = latestEntryDenominatorBundle[i];
                var earliestExitNumeratorCandidate = earliestExitNumeratorBundle[i];
                var earliestExitDenominatorCandidate = earliestExitDenominatorBundle[i];
                if (latestEntryNumeratorCandidate * latestEntryDenominator > latestEntryNumerator * latestEntryDenominatorCandidate)
                {
                    latestEntryNumerator = latestEntryNumeratorCandidate;
                    latestEntryDenominator = latestEntryDenominatorCandidate;
                    latestEntryIndex = latestEntryIndexBundle[i];
                }
                if (earliestExitNumeratorCandidate * earliestExitDenominator < earliestExitNumerator * earliestExitDenominatorCandidate)
                {
                    earliestExitNumerator = earliestExitNumeratorCandidate;
                    earliestExitDenominator = earliestExitDenominatorCandidate;
                }
            }
            // 如果最早的出口在原点后面,就没有命中。
            // 如果最早的出口出现在最晚的进入之前,则不会出现命中。
            // 入场分母为负,出场分母为正。需要比较号翻转。
            if (earliestExitNumerator < 0 ||
                latestEntryNumerator * earliestExitDenominator < earliestExitNumerator * latestEntryDenominator)
            {
                t = default;
                normal = default;
                return false;
            }
            else
            {
                t = latestEntryNumerator / latestEntryDenominator;
                if (t < 0)
                    t = 0;
                BundleIndexing.GetBundleIndices(latestEntryIndex, out var bundleIndex, out var innerIndex);
                Vector3Wide.ReadSlot(ref BoundingPlanes[bundleIndex].Normal, innerIndex, out normal);
                Matrix3x3.Transform(normal, orientation, out normal);
                return true;
            }

        }
        public void Dispose(BufferPool bufferPool)
        {
            bufferPool.Return(ref Points);
            bufferPool.Return(ref BoundingPlanes);
            bufferPool.Return(ref FaceVertexIndices);
            bufferPool.Return(ref FaceToVertexIndicesStart);
        }


        /// <summary>
        /// 凸面外壳形状的类型ID。
        /// </summary>
        public const int Id = 5;
        public int TypeId { [MethodImpl(MethodImplOptions.AggressiveInlining)] get { return Id; } }
    }

    public struct ConvexHullWide : IShapeWide<ConvexHull>
    {
        // 与其他形状不同,单个凸面外壳是内部矢量化的。
        // "宽"变量只是凸壳实例的集合。
        public Buffer<ConvexHull> Hulls;

        public int MinimumWideRayCount => int.MaxValue; // 不管怎样,"宽"射线测试只会落入标量测试。

        public bool AllowOffsetMemoryAccess => false;
        public int InternalAllocationSize => Vector<float>.Count * Unsafe.SizeOf<ConvexHull>();
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Initialize(in RawBuffer memory)
        {
            Debug.Assert(memory.Length == InternalAllocationSize);
            Hulls = memory.As<ConvexHull>();
        }

        public void Broadcast(in ConvexHull shape)
        {
            for (int i = 0; i < Hulls.Length; ++i)
                Hulls[i] = shape;
        }

        public void GetBounds(ref QuaternionWide orientations, int countInBundle, out Vector<float> maximumRadius, out Vector<float> maximumAngularExpansion, out Vector3Wide min, out Vector3Wide max)
        {
            maximumRadius = default; max = default; min = default;
            for (int i = 0; i < countInBundle; ++i)
            {
                Vector3Wide.Broadcast(new Vector3(float.MaxValue), out var minWide);
                Vector3Wide.Broadcast(new Vector3(float.MinValue), out var maxWide);
                QuaternionWide.Rebroadcast(orientations, i, out var orientationWide);
                Matrix3x3Wide.CreateFromQuaternion(orientationWide, out var orientationMatrix);
                Vector<float> maximumRadiusSquaredWide = default;
                ref var hull = ref Hulls[i];
                for (int j = 0; j < hull.Points.Length; ++j)
                {
                    ref var localPoint = ref hull.Points[j];
                    Matrix3x3Wide.TransformWithoutOverlap(localPoint, orientationMatrix, out var p);
                    Vector3Wide.LengthSquared(localPoint, out var lengthSquared);
                    maximumRadiusSquaredWide = Vector.Max(lengthSquared, maximumRadiusSquaredWide);
                    Vector3Wide.Min(minWide, p, out minWide);
                    Vector3Wide.Max(maxWide, p, out maxWide);
                }
                Vector3Wide.ReadFirst(minWide, out var minNarrow);
                Vector3Wide.ReadFirst(maxWide, out var maxNarrow);
                float maximumRadiusSquared = maximumRadiusSquaredWide[0];
                for (int j = 1; j < Vector<float>.Count; ++j)
                {
                    // TODO：检查codegen。边界检查被取消了吗？
                    var minCandidate = new Vector3(minWide.X[j], minWide.Y[j], minWide.Z[j]);
                    var maxCandidate = new Vector3(maxWide.X[j], maxWide.Y[j], maxWide.Z[j]);
                    minNarrow = Vector3.Min(minCandidate, minNarrow);
                    maxNarrow = Vector3.Max(maxCandidate, maxNarrow);

                    var maxRadiusCandidate = maximumRadiusSquaredWide[j];
                    if (maxRadiusCandidate > maximumRadiusSquared)
                        maximumRadiusSquared = maxRadiusCandidate;
                }
                GatherScatter.Get(ref maximumRadius, i) = maximumRadiusSquared;
                Vector3Wide.WriteSlot(minNarrow, i, ref min);
                Vector3Wide.WriteSlot(maxNarrow, i, ref max);
            }
            maximumRadius = Vector.SquareRoot(maximumRadius);
            // 请注意,这是一个非常保守的选择。您可以枚举这组面平面以获得真正的最小半径。
            // 这个函数没有麻烦-它可能值得缓存它和形状本身的最大半径。
            maximumAngularExpansion = maximumRadius;
        }

        public void RayTest(ref RigidPoses poses, ref RayWide rayWide, out Vector<int> intersected, out Vector<float> t, out Vector3Wide normal)
        {
            intersected = default; normal = default; t = default;
            Debug.Assert(Hulls.Length > 0 && Hulls.Length <= Vector<float>.Count);
            for (int i = 0; i < Hulls.Length; ++i)
            {
                RigidPoses.ReadFirst(GatherScatter.GetOffsetInstance(ref poses, i), out var pose);
                ref var offsetRay = ref GatherScatter.GetOffsetInstance(ref rayWide, i);
                Vector3Wide.ReadFirst(offsetRay.Origin, out var origin);
                Vector3Wide.ReadFirst(offsetRay.Direction, out var direction);
                var intersectedNarrow = Hulls[i].RayTest(pose, origin, direction, out var tNarrow, out var normalNarrow);

                GatherScatter.Get(ref intersected, i) = intersectedNarrow ? -1 : 0;
                GatherScatter.Get(ref t, i) = tNarrow;
                Vector3Wide.WriteSlot(normalNarrow, i, ref normal);
            }
        }

        /// <summary>
        /// 提供形状比例的估计值。
        /// </summary>
        /// <param name="telatedLanes">非活动车道的掩码。</param>
        /// <param name="epsilonScale">在epsilon中使用的形状的大致比例。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void EstimateEpsilonScale(in Vector<int> terminatedLanes, out Vector<float> epsilonScale)
        {
            Vector3Wide bundle = default;
            for (int i = 0; i < Vector<float>.Count; ++i)
            {
                if (terminatedLanes[i] < 0)
                    continue;
                Debug.Assert(Hulls.Length > i);
                Vector3Wide.CopySlot(ref Hulls[i].Points[0], 0, ref bundle, i);
            }
            epsilonScale = (Vector.Abs(bundle.X) + Vector.Abs(bundle.Y) + Vector.Abs(bundle.Z)) * new Vector<float>(1f / 3f);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WriteFirst(in ConvexHull source)
        {
            Hulls[0] = source;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WriteSlot(int index, in ConvexHull source)
        {
            Hulls[index] = source;
        }
    }

    public struct ConvexHullSupportFinder : ISupportFinder<ConvexHull, ConvexHullWide>
    {
        public bool HasMargin => false;

        // [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ComputeLocalSupport(in ConvexHullWide shape, in Vector3Wide direction, in Vector<int> terminatedLanes, out Vector3Wide support)
        {
            support = default;
            Helpers.FillVectorWithLaneIndices(out var indexOffsets);
            for (int slotIndex = 0; slotIndex < Vector<float>.Count; ++slotIndex)
            {
                if (terminatedLanes[slotIndex] < 0)
                    continue;
                ref var hull = ref shape.Hulls[slotIndex];
                Debug.Assert(hull.Points.Allocated, "If the lane isn't terminated, then the hull should actually exist. Did you forget to create a mask based on the bundle local count?");
                Vector3Wide.Rebroadcast(direction, slotIndex, out var slotDirection);
                var bestIndices = indexOffsets;
                Vector3Wide.Dot(slotDirection, hull.Points[0], out var dot);
                for (int j = 1; j < hull.Points.Length; ++j)
                {
                    ref var candidate = ref hull.Points[j];
                    Vector3Wide.Dot(slotDirection, candidate, out var dotCandidate);
                    var useCandidate = Vector.GreaterThan(dotCandidate, dot);
                    bestIndices = Vector.ConditionalSelect(useCandidate, indexOffsets + new Vector<int>(j << BundleIndexing.VectorShift), bestIndices);
                    dot = Vector.ConditionalSelect(useCandidate, dotCandidate, dot);
                }
                // 这个水平阶段实际上是一笔不小的成本;平台内部特性可能会提供一些潜在的改进。
                var bestSlotIndex = 0;
                var bestSlotDot = dot[0];
                for (int j = 1; j < Vector<float>.Count; ++j)
                {
                    var candidate = dot[j];
                    if (candidate > bestSlotDot)
                    {
                        bestSlotDot = candidate;
                        bestSlotIndex = j;
                    }
                }
                var supportIndex = bestIndices[bestSlotIndex];
                BundleIndexing.GetBundleIndices(supportIndex, out var bundleIndex, out var innerIndex);
                Vector3Wide.CopySlot(ref hull.Points[bundleIndex], innerIndex, ref support, slotIndex);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ComputeSupport(in ConvexHullWide shape, in Matrix3x3Wide orientation, in Vector3Wide direction, in Vector<int> terminatedLanes, out Vector3Wide support)
        {
            Matrix3x3Wide.TransformByTransposedWithoutOverlap(direction, orientation, out var localDirection);
            ComputeLocalSupport(shape, localDirection, terminatedLanes, out var localSupport);
            Matrix3x3Wide.TransformWithoutOverlap(localSupport, orientation, out support);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetMargin(in ConvexHullWide shape, out Vector<float> margin)
        {
            margin = default;
        }
    }
}
